use specs::{world::Index, ReadStorage, WriteStorage, System, Entities};
use crate::res::{
    position_storage::Position,
    observing_storage::Observing,
    overlord_storage::Overlord,
};

const MAX_ENTITIES_IN_LEAF: usize = 3;
const UNINITIALIZED_POS_INDEX: (Position, Index) = (Position{x: 0.0, y:0.0}, 0);
const VIEW_RANGE: f32 = 50f32;

#[derive(Copy, Clone, Debug)]
struct AABB {
    center_x: f32,
    center_y: f32,
    span_x: f32,
    span_y: f32,
}

#[derive(Clone, Debug)]
enum QuadTreeState {
    Node([Box<QuadNode>; 4]),
    Leaf(usize, [(Position, Index); MAX_ENTITIES_IN_LEAF]),
    Nil,
}

#[derive(Clone, Debug)]
struct QuadNode {
    bound: AABB,
    state: QuadTreeState,
}

fn create_children(bound: AABB) -> [Box<QuadNode>; 4] {
    return [
        Box::new(QuadNode { 
            bound: AABB {
                center_x: bound.center_x - (bound.span_x / 2f32),
                center_y: bound.center_y - (bound.span_y / 2f32),
                span_x: bound.span_x / 2f32,
                span_y: bound.span_y / 2f32,
            },
            state: QuadTreeState::Nil,
        }),
        Box::new(QuadNode { 
            bound: AABB {
                center_x: bound.center_x + (bound.span_x / 2f32),
                center_y: bound.center_y - (bound.span_y / 2f32),
                span_x: bound.span_x / 2f32,
                span_y: bound.span_y / 2f32,
            },
            state: QuadTreeState::Nil,
        }),
        Box::new(QuadNode { 
            bound: AABB {
                center_x: bound.center_x - (bound.span_x / 2f32),
                center_y: bound.center_y + (bound.span_y / 2f32),
                span_x: bound.span_x / 2f32,
                span_y: bound.span_y / 2f32,
            },
            state: QuadTreeState::Nil,
        }),  
        Box::new(QuadNode { 
            bound: AABB {
                center_x: bound.center_x + (bound.span_x / 2f32),
                center_y: bound.center_y + (bound.span_y / 2f32),
                span_x: bound.span_x / 2f32,
                span_y: bound.span_y / 2f32,
            },
            state: QuadTreeState::Nil,
        }),
    ];
}

fn add(node: QuadNode, position: Position, entity_id: Index) -> QuadNode {

    if let QuadTreeState::Nil = node.state {
        let mut array: [(Position, Index); MAX_ENTITIES_IN_LEAF] = [UNINITIALIZED_POS_INDEX; MAX_ENTITIES_IN_LEAF];
        array[0] = (position, entity_id);
        return QuadNode {
            bound: node.bound,
            state: QuadTreeState::Leaf(1, array),
        };
    }

    if let QuadTreeState::Leaf(index, array) = node.state {
        if index == MAX_ENTITIES_IN_LEAF {
            let mut children = create_children(node.bound);

            let mut extended_array = [UNINITIALIZED_POS_INDEX; MAX_ENTITIES_IN_LEAF+1];
            for i in 0..MAX_ENTITIES_IN_LEAF {
                extended_array[i] = array[i];
            }
            extended_array[MAX_ENTITIES_IN_LEAF] = (position, entity_id);

            for i in 0..MAX_ENTITIES_IN_LEAF+1 {
                let (pos, entity_id) = extended_array[i];
                let id = ((pos.x > node.bound.center_x) as usize) + 2*((pos.y > node.bound.center_y) as usize);
                children[id] = Box::new(add(*children[id].clone(), pos, entity_id));
            }

            return QuadNode {
                bound: node.bound,
                state: QuadTreeState::Node(children),
            };
        }

        let mut new_array: [(Position, Index); MAX_ENTITIES_IN_LEAF] = [UNINITIALIZED_POS_INDEX; MAX_ENTITIES_IN_LEAF];;
        for i in 0..index {
            new_array[i] = array[i];
        }
        new_array[index] = (position, entity_id);

        return QuadNode {
            bound: node.bound,
            state: QuadTreeState::Leaf(index + 1, new_array),
        };
    }

    if let QuadTreeState::Node(children) = node.state {
        let mut children = children.clone();
        let id = ((position.x > node.bound.center_x) as usize) + 2*((position.y > node.bound.center_y) as usize);
        children[id] = Box::new(add(*children[id].clone(), position, entity_id));

        return QuadNode {
            bound: node.bound,
            state: QuadTreeState::Node(children),
        };
    }
    
    QuadNode { bound: node.bound, state: QuadTreeState::Nil }
}

fn query(node: QuadNode, aabb: AABB) -> Vec<(Position, Index)> {
    let aabb_min = [
        aabb.center_x - aabb.span_x,
        aabb.center_y - aabb.span_y,
    ];

    let aabb_max = [
        aabb.center_x + aabb.span_x,
        aabb.center_y + aabb.span_y,
    ];

    let min = [
        node.bound.center_x - node.bound.span_x,
        node.bound.center_y - node.bound.span_y,
    ];

    let max = [
        node.bound.center_x + node.bound.span_x,
        node.bound.center_y + node.bound.span_y,
    ];

    let collision = min[0] < aabb_max[0] &&
        max[0] > aabb_min[0] &&
        min[1] < aabb_max[1] &&
        max[1] > aabb_min[1];

    if !collision {
        return Vec::new();
    }

    if let QuadTreeState::Leaf(index, array) = node.state {
        let mut ret = Vec::new();
        for i in 0..index {
            let (pos, entity_id) = array[i];
            if pos.x > aabb_min[0] && pos.x < aabb_max[0]
                && pos.y > aabb_min[1] && pos.y < aabb_max[1] {
                ret.push((pos, entity_id));
            }
        }

        return ret;
    }

    if let QuadTreeState::Node(children) = node.state {
        let mut ret = Vec::new();
        for child in children.iter() {
            ret.extend(query(*child.clone(), aabb).iter().clone());
        }

        return ret;
    }

    return Vec::new();
}

pub struct ObserveSystem;

impl<'a> System<'a> for ObserveSystem {
    type SystemData = (
        Entities<'a>,
        ReadStorage<'a, Position>,
        WriteStorage<'a, Observing>,
        ReadStorage<'a, Overlord>,
    );

    fn run(&mut self, data: Self::SystemData) {
        use specs::Join;

        let (entities, position, mut observing, overlords) = data;

        let mut root = QuadNode {
            bound: AABB {
                center_x: 0f32,
                center_y: 0f32,
                span_x: 500f32,
                span_y: 500f32,
            },
            state: QuadTreeState::Nil,
        };

        for (ent, pos) in (&*entities, &position).join() {
            root = add(root, *pos, ent.id());
        }

        for o in (&mut observing).join() {
            o.0.clear();
        }

        for (ent, pos) in (&*entities, &position).join() {
            if let Some(overlord) = overlords.get(ent) {
                let overlord_entity = entities.entity(overlord.0);

                if let Some(observing) = observing.get_mut(overlord_entity) {
                    let owned_entity_aabb = AABB {
                        center_x: pos.x,
                        center_y: pos.y,
                        span_x: VIEW_RANGE,
                        span_y: VIEW_RANGE,
                    };
                    let viewed_by_owned_entity = query(root.clone(), owned_entity_aabb);
                    
                    // This is an array full of overlords. The entity they own correspondes to
                    // the same index on "viewed_by_owned_entity" array.
                    let overlord_of_viewed_entities = viewed_by_owned_entity.clone()
                        .into_iter()
                        .map(|e| overlords.get(entities.entity(e.1)))
                        .collect::<Vec<Option<&Overlord>>>();
                    
                    // here we filter out every entity, which is owned by the overlord of the
                    // currently scouting entitiy (ent)
                    let mut viewed_by_owned_entity = viewed_by_owned_entity
                        .into_iter()
                        .zip(overlord_of_viewed_entities.into_iter())
                        .filter(|(_, overlord_option)| {
                            if let Some(overlord) = overlord_option {
                                overlord.0 != overlord_entity.id()
                            } else {
                                true
                            }
                        })
                        .map(|(e, _)| e.1)
                        .collect();

                    observing.0.append(&mut viewed_by_owned_entity);
                }
            }
        }
    }
}